#include "StdAfx.h"
#include "u_t.h"


u_t::u_t(){
}
u_t::u_t(double Rn,double Ic,double C, double Idc,double U0, double Iac, double w,long double h):rscjprops(Rn,Ic,C,w)
{
	this->error=1e-2;
	this->h=h;
	smax=500;
	this->Idc=Idc;
	this->Iac=Iac;
	this->U0=U0;
	if(Ic!=0){
	this->idc=Idc/Ic;
	this->iac=Iac/Ic;
	if(Rn!=0){
	this->u0=U0/(Ic*Rn);
	
	}
	}
	this->deltatmax=0.1;
	this->deltatmin=1e-5;
	}

u_t::u_t(double betac, double idc,double iac,double u0, double omega,long double h):rscjprops(betac,omega)
{
	this->error=1e-2;
	this->h=h;
	smax=500;
	this->Idc=-1.0;
	this->Iac=-1.0;
	this->U0=-1.0;
	this->idc=idc;
	this->iac=iac;
	this->u0=u0;
	this->deltatmax=0.1;
	this->deltatmin=1e-5;
}


u_t::~u_t(void)
{
}

void u_t::work(void){
	

	
	int n=2*smax/h;
	t.clear();
	v.clear();
	phi.clear();
		t.resize(1);
		v.resize(1);
		phi.resize(1);
		t.reserve(n);
		v.reserve(n);
		phi.reserve(n);
		t[0]=0;
		v[0]=u0;
		
		phi[0]=0;
	//this->rk4(this->t,this->v,this->phi,this->h);
	
	this->rk4_dynstep(this->t,this->v,this->phi,this->h);
	this->integrate();
}

void u_t::setIdc(double Idc){
	this->Idc=Idc;
	if(Ic>0){
	this->idc=Idc/(this->Ic);
	}
	this->normalized=false;
}
void u_t::setidc(double idc){
	this->Idc=-1;
	this->idc=idc;
	this->normalized=true;
}
void u_t::setU0(long double U0){
	this->U0=U0;
	if(Ic>0 && Rn>0){
		this->U0=U0/(Ic*Rn);
	}
	this->normalized=false;
}
void u_t::setu0(long double u0){
	this->u0=u0;
	this->U0=-1;
	this->normalized=false;
}
void u_t::setsmax(long double smax){
	this->smax=smax;
}
void u_t::seterror(double error){
	this->error=error;
}
void u_t::setdeltatmax(double tmax){
	this->deltatmax=tmax;
}

double u_t::getIdc(){
	return this->Idc;
}
double u_t::getidc(){
	return this->idc;
}
double u_t::getU(){
	return this->U;
}
double u_t::getu(){
	return this->u;
}
long double u_t::getU0(){
	return this->U0;
}
long double u_t::getu0(){
	return this->u0;
}
double u_t::getsmax(){
	return smax;
}
double u_t::getuerr(){
	return fabs(u-utemp);
}

//void u_t::integrate(){
//                long double Utemp=0;
//                int n=this->v.size();
//                int n2=n/2;
//                long double tmax=t.at(n2);
//                
//        
//        for(int i=0;i<n2-1;++i){
//                Utemp+=(this->t[i+1]-this->t[i])*this->v[i];
//        }
//        this->utemp=Utemp/tmax;
//        tmax=this->t.back();
//        for(int i=n2-1;i<n-1;++i){
//                Utemp+=(this->t[i+1]-this->t[i])*this->v[i];
//        }
//        this->u=Utemp/tmax;
//        if(!this->normalized){
//        this->U=this->u*(Ic*Rn);
//        }
//}
void u_t::integrate(){
                long double Utemp=0;
                int n=this->v.size();
                long double tmax=this->t.back();            
        for(int i=0;i<n-1;++i){
                Utemp+=(this->t[i+1]-this->t[i])*this->v[i];
        }
        this->u=Utemp/tmax;
        if(!this->normalized){
        this->U=this->u*(Ic*Rn);
        }
}

long double u_t::fkt(long double& y,long double& yt){
	return ((1.0/this->betac)*(this->idc-y-sin(yt)));
}
long double u_t::fkt(long double& t,long double& y,long double& yt){
	return ((1.0/this->betac)*(this->idc+this->iac*sin(this->omega*t)-y-sin(yt)));
}
void u_t::rk4_step(long double& h,long double& t,long double& v,long double& phi){
	long double v0,phi0,va,vb,vc,dv0,dva,dvb,dvc,phia,phib,phic,dphi0,dphia,dphib,dphic;
			long double t0=t;
				v0=v;
				phi0=phi;
				dphi0=v0;
				dv0=fkt(t0,v0,phi0);
				phia=phi0+h/2.0*dphi0;
				va=v0+h/2.0*dv0;
				t0+=h/2.0;
				dphia=va;
				dva=fkt(t0,va,phia);
				phib=phi0+h/2.0*dphia;
				vb=v0+h/2.0*dva;
				dphib=vb;
				dvb=fkt(t0,vb,phib);
				phic=phi0+h*dphib;
				t0+=h/2.0;
				vc=v0+h*dvb;
				dphic=vc;
				dvc=fkt(t0,vc,phic);
				
				v=(v0+h/6.0*(dv0+2.0*(dva+dvb)+dvc));
				phi=(phi0+h/6.0*(dphi0+2.0*(dphia+dphib)+dphic));
				//std::cout<<h<<" "<<betac<<" "<<omega<<" "<<idc<<" "<<iac<<" "<<phi0<<" "<<dphi0<<" "<<dphia<<" "<<dphib<<" "<<dphic<<std::endl;
			
}

void u_t::rk4(std::vector<long double>& t,std::vector<long double>& v,std::vector<long double>& phi,long double& h){
//long double v0,phi0,va,vb,vc,dv0,dva,dvb,dvc,phia,phib,phic,dphi0,dphia,dphib,dphic;
//		long double t0=0;
//		int i=0;
//		while(t.back()<smax){
//			v0=v.back();
//			phi0=phi.back();
//			dphi0=v0;
//			dv0=fkt(dc1,t0,v0,phi0);
//			phia=phi0+h/2.0*dphi0;
//			va=v0+h/2.0*dv0;
//			t0+=h/2.0;
//			dphia=va;
//			dva=fkt(dc1,t0,va,phia);
//			phib=phi0+h/2.0*dphia;
//			vb=v0+h/2.0*dva;
//			dphib=vb;
//			dvb=fkt(dc1,t0,vb,phib);
//			phic=phi0+h*dphib;
//			t0+=h/2.0;
//			vc=v0+h*dvb;
//			dphic=vc;
//			dvc=fkt(dc1,t0,vc,phic);
//			++i;
//			t.push_back(t.back()+h);
//			v.push_back(v0+h/6.0*(dv0+2.0*(dva+dvb)+dvc));
//			phi.push_back(phi0+h/6.0*(dphi0+2.0*(dphia+dphib)+dphic));
//		}

			long double ttemp,vtemp,phitemp=0;
			while(this->t.back()<this->smax){
				ttemp=this->t.back();
				vtemp=this->v.back();
				phitemp=this->phi.back();
				rk4_step(h,ttemp,vtemp,phitemp);
				this->t.push_back(ttemp+h);
				this->v.push_back(vtemp);
				this->phi.push_back(phitemp);
			}

}

void u_t::rk4_dynstep(std::vector<long double>& t,std::vector<long double>& v,std::vector<long double>& phi,long double& h){
	long double v0,phi0,va,vb,vc,dv0,dva,dvb,dvc,phia,phib,phic,dphi0,dphia,dphib,dphic;
	long double vtemp,vtemp2,vtemp22=0;
	long double phitemp,phitemp2,phitemp22=0;
	long double verror,phierror=0.0;
	long double t0=0;
	long double scale,scaleV,scalephi=0;
	bool err=true;
	long double h2=0;
	long double ttemp=0;
	long double htemp=0;
	int err_count=0;
			
			//int i=0;
			while(t.back()<smax){
				err=true;
				//i=0;
				t0=this->t.back();
				v0=this->v.back();
				phi0=this->phi.back();
				dphi0=v0;
				
				//dv0=this->fkt(t0,v0,phi0);//first step in runge kutta can be reused
				//scale=2;
				err_count=0;
				while((err)&&(err_count<4)){
					h2=2.0*h;
					vtemp=v0;
					phitemp=phi0;
					ttemp=t0;
					this->rk4_step(h2,ttemp,vtemp,phitemp);
					vtemp2=v0;
					phitemp2=phi0;
					this->rk4_step(h,ttemp,vtemp2,phitemp2);
					vtemp22=vtemp2;
					phitemp22=phitemp2;
					ttemp+=h;
					this->rk4_step(h,ttemp,vtemp22,phitemp22);


				//t0=t.back();
				//	if(i==0){	//first run =>has to be calculated
				//////begin steping with double stepsize
				//phia=phi0+h*dphi0;
				//va=v0+h*dv0;
				//dphia=va;
				//t0+=h;
				//dva=fkt(dc1,t0,va,phia);
				//phib=phi0+h*dphia;
				//vb=v0+h*dva;
				//dphib=vb;
				//dvb=fkt(dc1,t0,vb,phib);
				//phic=phi0+2.0*h*dphib;
				//vc=v0+2.0*h*dvb;
				//t0+=h;
				//dphic=vc;
				//dvc=fkt(dc1,t0,vc,phic);
				//vtemp=v0+h/3.0*(dv0+2.0*(dva+dvb)+dvc);
				//phitemp=phi0+h/3.0*(dphi0+2.0*(dphia+dphib)+dphic);//saving result for double stepsize
				//}else{	//not the first run can be taken from the last run
				//	vtemp=vtemp2;
				//	phitemp=phitemp2;
				//}
				////begin stepping for normal stepsize
				//phia=phi0+h/2.0*dphi0;	//dphi0 can be reused
				//va=v0+h/2.0*dv0;		//dv0 can also be reused
				//t0=t.back()+h/2.0;
				//dphia=va;
				//dva=fkt(dc1,t0,va,phia);
				//phib=phi0+h/2.0*dphia;
				//vb=v0+h/2.0*dva;
				//dphib=vb;
				//dvb=fkt(dc1,t0,vb,phib);
				//phic=phi0+h*dphib;
				//vc=v0+h*dvb;
				//t0+=h/2.0;
				//dphic=vc;
				//dvc=fkt(dc1,t0,vc,phic);
				//vtemp2=v0+h/6.0*(dv0+2.0*(dva+dvb)+dvc);
				//phitemp2=phi0+h/6.0*(dphi0+2.0*(dphia+dphib)+dphic);//saving results for first step with normal stepsize

				//////begin second step with normal stepsize
				//////v0 equals vtemp2 =>dphi0 equals vtemp2
				//////phi0 equals phitemp2
				//dv0=fkt(dc1,t0,vtemp2,phitemp2);
				//phia=phitemp2+h/2.0*vtemp2;	
				//va=vtemp2+h/2.0*dv0;
				//t0+=h/2.0;
				//dphia=va;
				//dva=fkt(dc1,t0,va,phia);
				//phib=phi0+h/2.0*dphia;
				//vb=v0+h/2.0*dva;
				//dphib=vb;
				//dvb=fkt(dc1,t0,vb,phib);
				//phic=phi0+h*dphib;
				//t0+=h/2.0;
				//vc=v0+h*dvb;
				//dphic=vc;
				//dvc=fkt(dc1,t0,vc,phic);
				//vtemp22=v0+h/6.0*(dv0+2.0*(dva+dvb)+dvc);
				//phitemp22=phi0+h/6.0*(dphi0+2.0*(dphia+dphib)+dphic);//saving results for second step with normal stepsize
				//				////calculating error
				//
				phierror=(fabs(phitemp22-phitemp));
				scale=phierror/(this->error+std::max(abs(phitemp22),abs(phitemp))*this->error);
				//verror=(fabs(vtemp22-vtemp));
				//scaleV=verror/(this->error+std::max(vtemp22,vtemp)*this->error);
				//scale=(scaleV+scalephi)/2.0;
				if(scale<1){
                                        err=false;
                                        
                                }else{
                                h=0.9*(h*(1.0/scale));
								++err_count;
                                }
                                //std::cout<<this->idc<<" "<<h<<" "<<phierror<<" "<<scale<<" "<<i<<" "<<this->error<<" "<<phitemp<<std::endl;
                                //++i;

								if(h>deltatmax){
									h=deltatmax;
								}
								if(h<deltatmin){
									h=deltatmin;
								}
                                }//end while(err)
                                
                                this->t.push_back(this->t.back()+h);
                                this->v.push_back(vtemp2);
                                this->phi.push_back(phitemp2);//saving first step
                                
                                this->t.push_back(this->t.back()+h);
                                this->v.push_back(vtemp22);
                                this->phi.push_back(phitemp22);//saving second step
                                htemp=0.9*(h*(1.0/scale));
                                if((htemp<deltatmax)&&(htemp>deltatmin) && (scale<0.9)&&(scale!=0)){
                                h=htemp;
                                }else if((h<deltatmax)&&(scale==0)){
                                        h=h*2.0;
                                }else{
									h=0.01;
								}
								if(h>deltatmax){
									h=deltatmax;
								}
								if(h<deltatmin){
									h=deltatmin;
								}
				//std::cout<<this->idc<<" "<<h<<" "<<phierror<<" "<<scale<<" "<<i<<std::endl;

			}
}

void u_t::rk5_dynstep(std::vector<long double>& t,std::vector<long double>& v,std::vector<long double>& phi,long double& h){
	long double c[7]={0,1.0/5.0,3.0/10.0,4.0/5.0,8.0/9.0,1.0,1.0};
	long double b[7]={35.0/384.0,0.0,500.0/1113.0,125.0/192.0,-2187.0/6784.0,11.0/84.0,0.0};
	long double b2[7]={5179.0/57600.0,0.0,7571.0/16695.0,393.0/640.0,-92097.0/339200.0,187.0/2100.0,1.0/40.0};
	long double a[7][6]={
		{0,0,0,0,0,0},
		{1.0/5.0,0,0,0,0,0},
		{3.0/40.0,9.0/40.0,0,0,0,0},
		{44.0/45.0,-56.0/15.0,32.0/9.0,0,0,0},
		{19372.0/6561.0,-25360.0/2187.0,64448.0/6561.0,-212.0/729.0,0,0},
		{9017.0/3168.0,-355.0/33.0,46732.0/5247.0,49.0/176.0,-5103.0/18656.0,0},
		{35.0/384.0,0,500.0/1113.0,125.0/192.0,-2187.0/6784.0,11.0/84.0}
	};
	long double k[2][7]={}; //where k[0][0] is v[i] and k[0][6] is v[i+1];k[1][0] is phi[i] and k[1][6] is phi[i+1]  
	long double ktemp[2]={0};
	long double k0[2]={0};
	long double k5[2]={0};
	long double k4[2]={0};
	long double t0=0;
	long double ttemp=0;
	bool stepping=true;
	long double error=1;
	long double scale=this->error;
	
	while(t0<this->smax){
	k0[0]=this->v.back();
	k0[1]=this->phi.back();
	t0=this->t.back();
	//while(stepping){
	for(int i=0;i<7;++i){
		ktemp[0]=k0[0];
		ktemp[1]=k0[1];
		
		for(int j=0;j<i;++j){
		ktemp[0]+=a[i][j]*k[0][j];
		ktemp[1]+=a[i][j]*k[1][j];
		//cout<<ktemp[0]<<endl;
		
		}
		
		ttemp=(t0+c[i]*h);
		k[0][i]=h*this->fkt(ttemp,ktemp[0],ktemp[1]);
		k[1][i]=h*k[0][i];
		//cout<<k[0][i]<<endl;

	}

	k5[0]=k0[0];
	k5[1]=k0[1];
	for(int i=0;i<6;++i){
		k5[0]+=b[i]*k[0][i];
		k5[1]+=b[i]*k[1][i];
	
	}
	//cout<<k5[0]<<endl;
	//k4[0]=k0[0];
	//k4[1]=k0[1];
	//for(int i=0;i<7;++i){
	//	k4[0]+=b2[i]*k[0][i];
	//	k4[1]+=b2[i]*k[1][i];
	//}
	//scale+=max(k5[1],k4[1])*relerror;
	////cout<<k5[1]<<" "<<k4[1]<<" "<<k5[0]<<" "<<k4[0]<<endl;
	//if(scale!=0){
	//	error=sqrt((pow((k5[0]-k4[0])/scale,2)+pow((k5[1]-k4[1])/scale,2))/2.0);
	//}else{
	//	error=0;
	//}
	//if(error<1){
	//	stepping=false;
	//}else if(h>deltatmin){
	////h=0.9*h*pow((1/error),(long double)0.5);
	//stepping=false;
	//}
	//

	//}//end while stepping

	this->t.push_back(t0+h);
	this->v.push_back(k5[0]);
	this->phi.push_back(k5[1]);

	/*if(error!=0 && h<deltatmax && error<0.9){
	h=0.9*h*pow((1/error),(long double)0.2);
	}else if(error==0 && h<deltatmax){
		h=h*2.0;
	}*/
	//long double temp=(k5[0]-k4[0]);
	//cout<<dc1<<" "<<h<<" "<<error<<" "<<scale<<" "<<endl;
	}//end while(t0<tmax)

}

